//
// File: RFNN_EKF_cal_v.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 20-Jun-2024 17:16:34
//

// Include Files
#include "RFNN_EKF_cal_v.h"
#include "EKF_weight_v3.h"
#include "RFNNpro_v.h"
#include <algorithm>
#include <cmath>

// Variable Definitions
static double a[32];

// Function Definitions
//
// Arguments    : double dg
//                double b_do
//                double sig
//                double sio
//                double v_o
//                double w_o
//                double *vL
//                double *vR
// Return Type  : void
//
void RFNN_EKF_cal_v(double dg, double b_do, double sig, double sio, double v_o,
                    double w_o, double *vL, double *vR)
{
  double a1[32];
  double b_dv[32];
  double Fnk[16];
  double S[4];
  double Outputs[2];
  double b;
  int S1_size[2];
  int a_tmp;
  // 输入向量
  S[0] = dg;
  S[1] = b_do;
  S[2] = sig;
  S[3] = sio;
  //  初始化静态变量
  // step end
  //
  // 神经网络部分计算输出
  b = ((dg + b_do) + sig) + sio;
  for (int i{0}; i < 32; i++) {
    b_dv[i] = a[i] * b;
  }
  RFNNpro_v(S, b_dv, Outputs, Fnk);
  *vL = Outputs[1] / (0.3 * std::abs(Outputs[1]) + 1.0);
  *vR = Outputs[0] / (0.3 * std::abs(Outputs[0]) + 1.0);
  //      V=(vL+vR)/2;
  //      vL=Outputs(1);
  //      vR=Outputs(2);
  // EKF修正
  if (b_do > 2.0) {
    S1_size[0] = 1;
    S1_size[1] = 2;
    S[0] = dg;
    S[1] = sig;
  } else {
    S1_size[0] = 1;
    S1_size[1] = 4;
    S[0] = dg;
    S[1] = b_do;
    S[2] = sig;
    S[3] = sio;
  }
  for (int i{0}; i < 16; i++) {
    a_tmp = i << 1;
    b_dv[i] = a[a_tmp];
    b_dv[i + 16] = a[a_tmp + 1];
  }
  EKF_weight_v3(S, S1_size, Fnk, Outputs, b_dv, v_o, w_o, a1);
  for (int i{0}; i < 16; i++) {
    a_tmp = i << 1;
    a[a_tmp] = a1[i];
    a[a_tmp + 1] = a1[i + 16];
  }
}

//
// Arguments    : void
// Return Type  : void
//
void RFNN_EKF_cal_v_init()
{
  static const double b_dv[32]{
      0.1057, 0.1776, 0.142,  0.3985, 0.1664, 0.1339, 0.6209, 0.0308,
      0.5737, 0.9391, 0.052,  0.3013, 0.9312, 0.2955, 0.7286, 0.3329,
      0.7378, 0.467,  0.0634, 0.6481, 0.8604, 0.0252, 0.9344, 0.8422,
      0.9843, 0.559,  0.8589, 0.854,  0.7855, 0.3478, 0.5133, 0.446};
  std::copy(&b_dv[0], &b_dv[32], &a[0]);
  //              a=[-0.027,0.19,-0.079,0.12,0.077,0.087,0.009,0.136,0.077,-0.017,0.17,-0.011,0.03,0.095,0.07,-0.014;
  //                  -0.061,0.06,0.065,0.164,0.052,0.007,0.001,0.208,0.216,0.016,0.2,-0.021,0.159,-0.006,0.125,-0.039];
  //              a=[0.16,0.205,0.193,0.15,0.081,0.109,0.046,0.122,0.063,0.023,0.22,0.132,0.13,0.081,0.11,0.121;
  //                  0.266,0.09,0.289,0.222,0.115,0.059,0.103,0.185,0.195,0.046,0.172,0.069,0.234,0.169,0.23,0.297];
  // VR上VL下
}

//
// File trailer for RFNN_EKF_cal_v.cpp
//
// [EOF]
//
